#pragma once

#include "muduo/base/ThreadPool.h"
#include "muduo/base/Timestamp.h"
#include "muduo/net/Buffer.h"

#include <interface/msg/images.hpp>
#include <opencv2/opencv.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>

#include <mutex>

namespace sensor::camera {

class CameraOneFrame;

class FrameStatistics {
public:
    void receivedOneFrame()
    {
        frames_time_record_.insert(muduo::Timestamp::now());
    }

    double fps()
    {
        auto iter = frames_time_record_.upper_bound(muduo::addTime(muduo::Timestamp::now(), -10));
        if (iter == frames_time_record_.end()) {
            return NAN;
        }
        frames_time_record_.erase(frames_time_record_.begin(), iter);
        if (frames_time_record_.size() < 2) {
            return NAN;
        }
        const auto tdiff = muduo::timeDifference((*frames_time_record_.rbegin()), (*frames_time_record_.begin()));
        return frames_time_record_.size() / tdiff;
    }

private:
    std::set<muduo::Timestamp> frames_time_record_;
};

class ImageCollector : public rclcpp::Node {
public:
    explicit ImageCollector(size_t devnum);

    void infuseOneFrame(const std::string& device, const std::shared_ptr<CameraOneFrame> frame);
    void reset();

private:
    void processOneFrame(int64_t now, const std::string& key, cv::Mat& in, std::set<sensor_msgs::msg::Image::SharedPtr>& out,
        muduo::CountDownLatch& cdl);
    void saveVideoFiles(const std::unordered_map<std::string, cv::Mat>& frames);

private:
    const size_t devnum_;
    rclcpp::Logger logger_;

    std::mutex frames_lock_;
    std::unordered_map<std::string, std::shared_ptr<CameraOneFrame>> frames_;

    std::unordered_map<std::string, FrameStatistics> fps_;
    std::unordered_map<std::string, FrameStatistics> aligned_fps_;

    rclcpp::Publisher<interface::msg::Images>::SharedPtr pub_;
    std::unique_ptr<muduo::ThreadPool> threads_;

    std::mutex video_writer_lock_;
    std::unordered_map<std::string, std::shared_ptr<cv::VideoWriter>> video_writers_;
    muduo::Timestamp last_video_saved_time_;

    int image_tx_counter_ = 0;
};

}
